//
//  orbitalElements.cpp
//  Space Explorer
//
//  Created by Yann Blaudin de Thé on 20/09/11.
//  Copyright 2011 EADS Astrium ST. All rights reserved.
//

#include "SEOrbitalElements.h"


SpiceDouble calc_eccentric_anomaly(SpiceDouble mean_ano, SpiceDouble ecc, SpiceDouble eps) {
    SpiceDouble ecc_ano_p, ecc_ano;
    
    ecc_ano = mean_ano;
    
    while (fabs(ecc_ano-ecc_ano_p) > eps) {
        ecc_ano_p = ecc_ano;
        
        ecc_ano = (ecc*sin(ecc_ano_p) - ecc*ecc_ano_p*cos(ecc_ano_p) + mean_ano)/(1-ecc*cos(ecc_ano_p));
    }
    
    return ecc_ano;
    
}

// http://naif.jpl.nasa.gov/pub/naif/toolkit_docs/FORTRAN/spicelib/oscelt.html
// http://naif.jpl.nasa.gov/pub/naif/toolkit_docs/FORTRAN/spicelib/conics.html

void get_orbital_elmnt(const SpiceDouble et, SpiceDouble mu, const btVector3& pos_j2000, const btVector3& vit_j2000, SEOrbitalElements* elmnt) {
    SpiceDouble pv[6];
    SpiceDouble spice_elts[8];
    SpiceDouble ecc_ano, e, e2, v, a, b, r, mean_ano, T, C;
    SpiceDouble mom[3];
    
    pv[0] = pos_j2000.x();
    pv[1] = pos_j2000.y();
    pv[2] = pos_j2000.z();
    
    pv[3] = vit_j2000.x();
    pv[4] = vit_j2000.y();
    pv[5] = vit_j2000.z();
    
    vcrss_c(pv, pv+3, mom);
    C = vnorm_c(mom);
    
    oscelt_c(pv, et, mu, spice_elts);
    r = spice_elts[0];
    
    elmnt->perifocalDistance = r;
    e = spice_elts[1];
    e2 = e*e;
    elmnt->eccentricity = e;
    elmnt->inclination = spice_elts[2];
    elmnt->ascendingNodeLongitude = spice_elts[3];
    elmnt->periapsisArgument = spice_elts[4];
    mean_ano = spice_elts[5];
    elmnt->meanAnomaly = mean_ano;
    elmnt->epoch = spice_elts[6];
    elmnt->GravitationalParameter = spice_elts[7];
    
    if (e >= 1.0f) {
        T = -1.0f;
    } else {
        T = M_PI*2/pow(1-e2,1.5)*pow(C,3)/pow(mu,2);
    }
    
    elmnt->orbitalPeriod = T;
    ecc_ano = calc_eccentric_anomaly(mean_ano, e);
//    http://en.wikipedia.org/wiki/True_anomaly
    v = 2*atan2(sqrt(1+e)*sin(ecc_ano/2),sqrt(1-e)*cos(ecc_ano/2));
    elmnt->trueAnomaly = v;
    
    a = r*(1+e*cos(v))/(1-e2);
    b = a*sqrt(1-e2);
    elmnt->semiMajorAxis = a;
    elmnt->semiMinorAxis = b;
    
    elmnt->linearEccentricity = a*e;
    elmnt->periapsisDistance = a*(1-e);
    elmnt->apoapsisDistance = a*(1+e);
//    http://en.wikipedia.org/wiki/Eccentric_anomaly
    elmnt->eccentricAnomaly = ecc_ano;
    elmnt->meanLongitude = mean_ano + elmnt->ascendingNodeLongitude + elmnt->periapsisArgument;
    elmnt->trueLongitude = v + elmnt->ascendingNodeLongitude + elmnt->periapsisArgument;
    
}
